classdef ParametricMagnetometer < handle
    %PARAMETRICMAGNETOMETER Summary of this class goes here
    %   Detailed explanation goes here
    
    properties
        session
        ziMaster
        ziSlave
        app
        
        channelDrv
        channelY
        channelZ
        
        dataSweepPhaseY
        dataSweepPhaseZ
        dataSweepBiasY
        dataSweepBiasZ        
    end
    
    methods
        function obj = ParametricMagnetometer(session)
            %PARAMETRICMAGNETOMETER Construct an instance of this class
            %   Detailed explanation goes here
            obj.session = session;
            
            ziDevNames = session.getDevice();
            if length(ziDevNames) >= 2
                obj.ziMaster = session.getDevice(ziDevNames{1});
                obj.ziSlave = session.getDevice(ziDevNames{2});
            end
            
            obj.channelDrv = '0';
            obj.channelY = '2';
            obj.channelZ = '3';
        end
        %%
        function showGUI(obj)
            ParametricMagnetometerAPP(obj); 
        end
        %% Set Conditions
        function obj = setMainModFreq(obj, freq)
            obj.ziMaster.setOscFreq(obj.channelDrv, freq); pause(1.0);
        end
        %% Find working point
        function wp = findWorkingPoint(obj, varargin)
            wp.frequency = obj.ziMaster.getOscFreq(obj.channelDrv);
            wp.amplitude = obj.ziMaster.getSigOutAmplitude(obj.channelDrv);
            
            p=inputParser;
            
            levelCheck = @(x) x > 0.0 && x < 10.0;
            biasRangeCheck = @(x) x > 0.0 && x < 1.0;
            phaseRangeCheck = @(x) x > 1 && x <= 180;

            p.addParameter('biasY', 1.0, levelCheck);
            p.addParameter('biasZ', 2.0, levelCheck);
            p.addParameter('biasRawRange', 0.5, biasRangeCheck);
            p.addParameter('biasFineRange', 0.2, biasRangeCheck);
            p.addParameter('phaseFineRange', 10, phaseRangeCheck);
            p.parse(varargin{:});
            
            [phaseY0, phaseZ0, biasY0, biasZ0] = obj.scanPhaseBias('biasY', p.Results.biasY, ...
                                                                   'biasZ', p.Results.biasZ, ...
                                                                   'biasRange', p.Results.biasRawRange, ...
                                                                   'phaseYRange', [0.0, 180.0], ...
                                                                   'phaseZRange', [0.0, 180.0], ...
                                                                   'name', 'RawScan' );

            phaseRange = p.Results.phaseFineRange;
            [phaseY, phaseZ, biasY, biasZ, stdY, stdZ, sseY, sseZ] = obj.scanPhaseBias('biasY', biasY0, ...
                                                               'biasZ', biasZ0, ...
                                                               'biasRange', p.Results.biasFineRange, ...
                                                               'phaseYRange', phaseY0 + [-phaseRange, phaseRange], ...
                                                               'phaseZRange', phaseZ0 + [-phaseRange, phaseRange], ...
                                                               'name', 'FineScan' );
            if stdY > 0.001 || stdZ > 0.001
                warning('Too large std error during phase sweeping.');
            end
            if sseY > 0.001 || sseZ > 0.001
                warning('Bad fitting during bias sweeping.');
            end
            
            wp.phaseY = phaseY; wp.phaseZ = phaseZ;
            wp.biasY = biasY; wp.biasZ = biasZ;
            wp.stdY = stdY; wp.stdZ = stdZ;
            wp.sseY = sseY; wp.sseZ = sseZ;
        end
        
        function [phaseY, phaseZ, biasY, biasZ, stdY, stdZ, sseY, sseZ] = scanPhaseBias(obj, varargin)
            p=inputParser;
            levelCheck = @(x) x > 0.0 && x < 10.0;
            biasRangeCheck = @(x) x > 0.0 && x < 1.0;
            phaseRangeCheck = @(x) length(x) == 2;
            pointCheck = @(x) x > 1 && x <= 200;
            nameCheck = @(x) any(ismember({'RawScan', 'FineScan'}, x));
            p.addParameter('biasY', 1.0, levelCheck);
            p.addParameter('biasZ', 2.0, levelCheck);
            p.addParameter('biasRange', 0.5, biasRangeCheck);
            p.addParameter('biasPoint', 20, pointCheck);
            p.addParameter('phaseYRange', [0 180], phaseRangeCheck);
            p.addParameter('phaseZRange', [0 180], phaseRangeCheck);
            p.addParameter('phasePoint', 50, pointCheck);
            p.addParameter('name', 'RawScan', nameCheck);
            p.parse(varargin{:});
            
            L = log4m.getLogger;
            
            scanName = p.Results.name;
            biasY = p.Results.biasY; 
            biasZ = p.Results.biasZ;
            biasRange = [-p.Results.biasRange, 0.0, p.Results.biasRange];
            nBias = p.Results.biasPoint;
            phaseYMin = p.Results.phaseYRange(1); 
            phaseYMax = p.Results.phaseYRange(2);
            phaseZMin = p.Results.phaseZRange(1); 
            phaseZMax = p.Results.phaseZRange(2);
            nPhase = p.Results.phasePoint;
            
            [phaseY, stdY] = obj.sweepPhaseY(biasY, biasZ + biasRange, phaseYMin, phaseYMax, nPhase);
            [phaseZ, stdZ] = obj.sweepPhaseZ(biasY + biasRange, biasZ, phaseZMin, phaseZMax, nPhase);
            L.info('ParametricMagnetometer::findWorkingPoint', sprintf('%s: phaseY = %5.3f, stdY = %5.3f; phaseZ = %5.3f, stdZ = %5.3f', scanName, phaseY, stdY, phaseZ, stdZ));
            
            [biasY, sseY] = obj.sweepBiasY(biasY+biasRange(1), biasY+biasRange(end), nBias);
            [biasZ, sseZ] = obj.sweepBiasZ(biasZ+biasRange(1), biasZ+biasRange(end), nBias);
            L.info('ParametricMagnetometer::findWorkingPoint', sprintf('%s biasY = %5.3f, sseY = %5.3f; biasZ = %5.3f, sseZ = %5.3f', scanName, biasY, sseY, biasZ, sseZ));
        end
        
        function [optPhase, minStd]  = sweepPhaseY(obj, biasY, biasZList, phaseMin, phaseMax, nPhase)
            obj.dataSweepPhaseY = arrayfun(@(biasZ) obj.sweepPhase(obj.channelY, biasY, biasZ,...
                                                               'minPhase', phaseMin, 'maxPhase', phaseMax, 'nPhase', nPhase),...
                                                               biasZList);
            [optPhase, minStd] = obj.getOptPhase(obj.dataSweepPhaseY, obj.channelY);
            obj.ziMaster.setDemodPhase(obj.channelY, optPhase);
        end
        function [optPhase, minStd]  = sweepPhaseZ(obj, biasYList, biasZ, phaseMin, phaseMax, nPhase)
            obj.dataSweepPhaseZ = arrayfun(@(biasY) obj.sweepPhase(obj.channelZ, biasY, biasZ, ...
                                                               'minPhase', phaseMin, 'maxPhase', phaseMax, 'nPhase', nPhase),...
                                                               biasYList);
            [optPhase, minStd]  = obj.getOptPhase(obj.dataSweepPhaseZ, obj.channelZ);
            obj.ziMaster.setDemodPhase(obj.channelZ, optPhase);
        end
        function [optBias, sse] = sweepBiasY(obj, minBiasY, maxBiasY, nBias)
            obj.dataSweepBiasY = obj.sweepBias(obj.channelY, minBiasY, maxBiasY, 'nBias', nBias);                        
            data = obj.dataSweepBiasY.getNodeData(obj.channelY, 'x');
            [~, optBias, ~, sse] = obj.linearFit(data.grid, data.value);
            obj.ziMaster.setAuxOutOffset(obj.channelY, optBias);
        end                
        function [optBias, sse] = sweepBiasZ(obj, minBiasZ, maxBiasZ, nBias)
            obj.dataSweepBiasZ = obj.sweepBias(obj.channelZ, minBiasZ, maxBiasZ, 'nBias', nBias);
            data = obj.dataSweepBiasZ.getNodeData(obj.channelZ, 'x');
            [~, optBias, ~, sse] = obj.linearFit(data.grid, data.value);
            obj.ziMaster.setAuxOutOffset(obj.channelZ, optBias);
        end
        
        function swpr_data = sweepPhase(obj, chSweep, biasY, biasZ, varargin)
            p=inputParser;
            p.addParameter('minPhase', 0.0);
            p.addParameter('maxPhase', 180.0);
            p.addParameter('nPhase', 50);
            p.parse(varargin{:});    

            zi = obj.ziMaster;
            
            chY = '2'; chZ = '3';     
            originalBiasY = zi.getAuxOutOffset(chY);
            originalBiasZ = zi.getAuxOutOffset(chZ);
            zi.setAuxOutOffset(chY, biasY);
            zi.setAuxOutOffset(chZ, biasZ);
            swpr=zi.sweepModule();

            swpr.setControl('gridnode',  ['demods/' chSweep '/phaseshift'], 'start', p.Results.minPhase, 'stop', p.Results.maxPhase, 'samplecount', p.Results.nPhase);
            swpr.setFilter('bandwidth', 300.0, 'order', 4);
            swpr.setSettling('settling_time', 0.0);
            swpr.setStatistics('averaging_sample', 10);

            swpr.reset();
            swpr.subscribeChannel(chSweep, {'x'});
            swpr_data = swpr.run();
            swpr.unsubscribeChannel();

            zi.setAuxOutOffset(chY, originalBiasY);
            zi.setAuxOutOffset(chZ, originalBiasZ);
        end
        
        function swpr_data = sweepBias(obj, chSweep, minBias, maxBias, varargin)
            p=inputParser;
            p.addParameter('nBias', 20);
            p.parse(varargin{:}); 
            
            zi = obj.ziMaster;
            
            swpr=zi.sweepModule();

            swpr.setControl('gridnode',  ['auxouts/' chSweep '/offset'], 'start', minBias, 'stop', maxBias, 'samplecount', p.Results.nBias);
            swpr.setFilter('bandwidth', 300.0, 'order', 4);
            swpr.setSettling('settling_time', 0.0);
            swpr.setStatistics('averaging_sample', 10);

            swpr.reset();
            swpr.subscribeChannel(chSweep, {'x'});
            swpr_data = swpr.run();
            swpr.unsubscribeChannel();
        end


    end
    
    methods 
        function [slope, intercept_x, intercept_y, sse] = linearFit(~, data_x, data_y)
            [fitRes, gof] = fit(data_x, data_y, 'poly1');
            slope = fitRes.p1;
            intercept_x = -fitRes.p2/fitRes.p1;
            intercept_y = fitRes.p2;
            sse = gof.sse;
        end
        function [optPhase, minStd] = getOptPhase(~, data, ch)
            if ~iscell(data)
                data = num2cell(data);
            end

            data_1 = data{1}.getNodeData(ch, 'x');
            phase = data_1.grid;

            val = zeros(length(data), length(phase));
            for k=1:length(data)
                data_k = data{k}.getNodeData(ch, 'x');
                val(k, :) = data_k.value;
            end
            [minStd, idx] = min(std(val));
            optPhase = phase(idx);

            if optPhase > 90.0
                optPhase = optPhase - 180.0;
            end
        end
    end
    
    methods
        function data = workingPoint_vs_MainFreq(obj, freqList)
            data = arrayfun(@(freq) obj.setMainModFreq(freq).findWorkingPoint(), freqList);
            obj.session.addMeasurementData('WorkingPointVsFrequency', data);
        end
    end
end

